#include "protocol_sensor.h"
#include <string.h>

#pragma pack(1)
typedef struct {
    uint8_t SensorID[6];
    uint8_t PacketType : 3;
    uint8_t FragInd : 1;
    uint8_t DataLen : 4;
    uint8_t Data[0];
} sensor_prtl_head_t;

typedef struct {
    uint8_t sseq:6;
    uint8_t flag:2;
    uint8_t pseq:7;
    uint8_t prior:1;
    uint16_t size;
    uint8_t data[0];
} sensor_prtl_frag_t;

typedef struct
{
    uint8_t RequestSetFlag:1;
    uint8_t CtrlType:7;
} sensor_prtl_ctrl_t;

typedef struct {
    uint16_t LengthFlag:2;
    uint16_t DataType:14;
    uint8_t  plen[0];
} sensor_data_head_t;
#pragma pack()

enum {
    SENSOR_PACK_TYPE_MON   = 0,         //监测报文
    SENSOR_PACK_TYPE_MONACK,            //监测响应
    SENSOR_PACK_TYPE_WARN,              //告警报文
    SENSOR_PACK_TYPE_WARNACK,           //告警响应
    SENSOR_PACK_TYPE_CTRL,              //控制报文
    SENSOR_PACK_TYPE_CTRLACK,           //控制响应
    SENSOR_PACK_TYPE_FRAGACK            //分片响应
};

enum {
    SENSOR_CTRL_TYPE_REQMON = 1,        //请求监测数据
    SENSOR_CTRL_TYPE_REQTIME,           //请求校时
    SENSOR_CTRL_TYPE_SETTIME,           //时间查询/设置
    SENSOR_CTRL_TYPE_SETCOMM,           //通用参数查询/设置
    SENSOR_CTRL_TYPE_SETWARN,           //告警参数查询/设置
    SENSOR_CTRL_TYPE_SETID,             //ID查询/设置
    SENSOR_CTRL_TYPE_RESET              //传感器复位
};

//0不支持 1:uint8 2:uint16 3:uint32 4:float
static const uint8_t gdatatype[] = {
    3,2,4,1,1,1,0,4,2,4,2,4,4,4,2,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,2,4,
    4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,0,4,4,4,0,4,4,4,4,4,4,0,4,4,4,
    2,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,1,4,1,4,2,4,4,4,4,2,4,2,2,4,4,4,4,4,4,
    4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,
    4,4,4,0,0,0,0,4,4,2,4,1,1,1,1,1,4,4,4,4,4,3,4,4,0,4,1,1,4,4,4,4,4,4,4,4,4,4
};

// -----------------------------------------------------------------------------
// DESCRIPTION: RTU CRC校验的高位字节表
// -----------------------------------------------------------------------------
static const unsigned char auchCRCHi[] = {
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80,
    0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
    0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
    0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
    0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00,
    0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80,
    0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01,
    0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80,
    0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40
};

// -----------------------------------------------------------------------------
// DESCRIPTION: RTU CRC校验的低位字节表
// -----------------------------------------------------------------------------
static const unsigned char auchCRCLo[] = {
    0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D,
    0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB,
    0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2,
    0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37,
    0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8,
    0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24,
    0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63,
    0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE,
    0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F,
    0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,
    0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C,
    0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89,
    0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46,
    0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40
};

/****
Global Functions
****/
/**
 ******************************************************************************
 ** \brief  系统初始化.
 **
 ** \param  sseq:sdu 序号  pseq:分片pdu序号
 ** \return none
 ******************************************************************************/
__weak void AppFragAckHandle(uint8_t sseq, uint8_t pseq);
__weak void AppWarnParamHandle(uint16_t param, double AlarmUpperLimit, double AlarmLowerLimit);
__weak void AppWarnAckHandle(bool success);
__weak void AppCommParamHandleU8(uint16_t param, const uint8_t* values, uint16_t len);
__weak void AppCommParamHandleU16(uint16_t param, const uint16_t* values, uint16_t len);
__weak void AppCommParamHandleU32(uint16_t param, const uint32_t* values, uint16_t len);
__weak void AppCommParamHandleFloat(uint16_t param, const float* values, uint16_t len);

//CRC校验函数：
uint16_t SENSOR_PRTL_CRC( const uint8_t * puchMsg, uint16_t  usDataLen )
{
    uint8_t  uchCRCHi;                // high byte of CRC initialized
    uint8_t  uchCRCLo;                // low byte of CRC initialized
    uint32_t uIndex;                       // will index into CRC lookup table

    uchCRCHi = 0xFF;
    uchCRCLo = 0xFF;

    while ( usDataLen-- )
    {
        // calculate the CRC
        uIndex = uchCRCLo ^ (unsigned char)( *puchMsg++ );
        uchCRCLo = uchCRCHi ^ auchCRCHi[uIndex];
        uchCRCHi = auchCRCLo[uIndex];
    }

    return ( uchCRCHi << 8 | uchCRCLo );
}


static uint16_t sSensorDataLen = 0;

//创建传感器规范监测和告警报文帧头
void SENSOR_PRTL_MakeHead(uint8_t* buf, uint8_t* id, bool warn)
{
    sensor_prtl_head_t* h = (sensor_prtl_head_t*)buf;

    memcpy(h->SensorID, id, 6);
    h->PacketType = warn? SENSOR_PACK_TYPE_WARN : SENSOR_PACK_TYPE_MON;

    sSensorDataLen = sizeof(sensor_prtl_head_t);
}

// 向监听或告警报文中增加参量数据 buf: 报文缓存，type: 参量类型，data: 参量数据
// dtype: 参量数据类型
bool SENSOR_PRTL_AddData(uint8_t* buf, uint16_t type, double data, uint8_t dtype)
{
    sensor_prtl_head_t* h = (sensor_prtl_head_t*)buf;
    sensor_data_head_t* dh = (sensor_data_head_t*)&buf[sSensorDataLen];

    switch(dtype)
    {
    case 0: //不支持的类型
        return false;
    case 1: //int8 单字节整形
    {
        uint8_t tmp = data;
        dh->DataType = type;
        dh->LengthFlag = 1;
        sSensorDataLen += sizeof(sensor_data_head_t);
        buf[sSensorDataLen++] = 1;
        buf[sSensorDataLen++] = tmp;
        break;
    }
    case 2: //int16 双字节整形
    {
        uint16_t tmp = data;
        dh->DataType = type;
        dh->LengthFlag = 1;
        sSensorDataLen += sizeof(sensor_data_head_t);
        buf[sSensorDataLen++] = 2;
        memcpy(buf + sSensorDataLen, &tmp, 2);
        sSensorDataLen += 2;
        break;
    }
    case 3: //int32 4字节整形
    {
        uint32_t tmp = data;
        dh->DataType = type;
        dh->LengthFlag = 0;
        sSensorDataLen += sizeof(sensor_data_head_t);
        memcpy(buf + sSensorDataLen, &tmp, 4);
        sSensorDataLen += 4;
        break;
    }
    case 4: //float 单精度浮点型
    {
        float fdata = data;
        dh->DataType = type;
        dh->LengthFlag = 0;
        sSensorDataLen += sizeof(sensor_data_head_t);
        memcpy(buf + sSensorDataLen, &fdata, 4);
        sSensorDataLen += 4;
        break;
    }
    default: //其它不支持的类型
        return false;
    }

    h->DataLen++;

    return true;
}

//增加分片数据 buf:报文缓存, type:参量类型, flag:分片标志, sseq:帧序号, 
//pseq:分片序号, data:参量数据数组缓存,dtype:参量数据类型, len:参量数据数组大小
//total_len:帧总长度
bool SENSOR_PRTL_AddFragData(uint8_t* buf, uint16_t type, uint8_t flag, 
                             uint8_t sseq, uint8_t pseq, const uint8_t* data, 
                             uint8_t dtype, uint16_t len, uint32_t total_len
                            )
{
    sensor_prtl_head_t* h = (sensor_prtl_head_t*)buf;

    switch(dtype)
    {
    case 0:
        return false;
    case 1: //int8 单字节整形
    {
        break;
    }
    case 2: //int16 双字节整形
    {
        if (total_len % 2 != 0 || len % 2 != 0)
            return false;

        break;
    }
    case 3: //int32 4字节整形
    case 4: //float 单精度浮点型
    {
        if (total_len % 4 != 0 || len % 4 != 0)
            return false;

        break;
    }
    default:
        return false;
    }

    sensor_prtl_frag_t* frag = (sensor_prtl_frag_t*)(buf + sSensorDataLen);
    sensor_data_head_t* dh = (sensor_data_head_t*)frag->data;

    frag->flag = flag;
    frag->sseq = sseq;
    frag->size = 0;

    sSensorDataLen += sizeof(sensor_prtl_frag_t);

    if (flag <= SENSOR_FLAG_FRAG_START)
    {
        dh->DataType = type;
        dh->LengthFlag = 3;
        memcpy(dh->plen, &total_len, 3);

        //sSensorDataLen += sizeof(sensor_data_head_t) + 3;
        frag->size += sizeof(sensor_data_head_t) + 3;
    }

    frag->pseq = pseq;

    memcpy(frag->data + frag->size, &data, len);
    //sSensorDataLen += len;
    frag->size += len;
    sSensorDataLen += frag->size;
    h->DataLen++;
    h->FragInd = 1;

    return true;
}

//缓存报文发送, 计算crc，发送报文
int SENSOR_PRTL_Send(uint8_t* buf, bool burst)
{
    if (sSensorDataLen <= sizeof(sensor_data_head_t))
        return -1;

    uint16_t crc = SENSOR_PRTL_CRC(buf, sSensorDataLen);
    buf[sSensorDataLen++] = crc >> 8;
    buf[sSensorDataLen++] = crc;

#ifdef SENSOR_MCU
    return AT_SendData(buf, sSensorDataLen, burst);
#else
    return nwk_senddata(burst, buf, sSensorDataLen);
#endif
}

const static uint8_t gParamLen[] = {0, 1, 2, 4, 4};
#define SENSOR_PARAM_LEN(paramtype) (gParamLen[gdatatype[paramtype-1]])

//检查数据内容
int SENSOR_PRTL_CheckDataCotent(sensor_prtl_head_t* h, const uint8_t* buf, uint16_t len, bool warn)
{
    uint16_t offset = 0;
    uint8_t params = 0;

    while( offset < len)
    {
        uint32_t l;
        sensor_data_head_t* dh = (sensor_data_head_t*)(buf + offset);

        //参量类型是否有效
        if (dh->DataType < 1 || dh->DataType > sizeof(gdatatype))
            return -1;

        offset += sizeof(sensor_data_head_t);

        switch(dh->LengthFlag)
        {
        case 0:
            l = 4;
            break;
        case 1:
            l = buf[offset++];
            break;
        case 2:
            memcpy(&l, buf + offset, 2);
            offset += 2;
            break;
        case 3:
            memcpy(&l, buf + offset, 3);
            offset += 3;
            break;
        }

        if (warn)
        {
            if ( l != (SENSOR_PARAM_LEN(dh->DataType) << 1) || l + offset > len)
                return -1;
        }
        else
        {
            if ( l % SENSOR_PARAM_LEN(dh->DataType) != 0)
                return -1;
        }

        offset += l;
        params++;

        if (params >= h->DataLen)
            break;
    }

    if (params != h->DataLen)
        return -1;

    return offset;
}

//设置告警参量
int SENSOR_PRTL_SetWarnParam(const uint8_t* buf, uint16_t len)
{
    uint16_t offset = 0;
    uint8_t params = 0;

    while( offset < len)
    {
        uint32_t l;
        sensor_data_head_t* dh = (sensor_data_head_t*)(buf + offset);

        //参量类型是否有效
        if (dh->DataType < 1 || dh->DataType > sizeof(gdatatype))
            return -1;

        switch(dh->LengthFlag)
        {
        case 0: //数据长度为4字节
            l = 4;
            break;
        case 1:
            l = buf[offset++];
            break;
        case 2:
            memcpy(&l, buf + offset, 2);
            offset += 2;
            break;
        case 3:
            memcpy(&l, buf + offset, 3);
            offset += 3;
            break;
        }

        if ( l != (SENSOR_PARAM_LEN(dh->DataType) << 1) || l + offset > len)
            return -1;

        double AlarmUpperLimit = 0;
        double AlarmLowerLimit = 0;

        switch(gdatatype[dh->DataType-1])
        {
        case 0:
            return -1;
        case 1:
        {
            AlarmUpperLimit = buf[offset++];
            AlarmLowerLimit = buf[offset++];

            break;
        }
        case 2:
        {
            uint16_t tmp;

            memcpy(&tmp, buf + offset, 2);
            AlarmUpperLimit = tmp;
            offset += 2;
            memcpy(&tmp, buf + offset, 2);
            AlarmLowerLimit = tmp;
            offset += 2;

            break;
        }
        case 3:
        {
            uint32_t tmp;

            memcpy(&tmp, buf + offset, 4);
            AlarmUpperLimit = tmp;
            offset += 4;
            memcpy(&tmp, buf + offset, 4);
            AlarmLowerLimit = tmp;
            offset += 4;

            break;
        }
        case 4:
        {
            float tmp;

            memcpy(&tmp, buf + offset, 4);
            AlarmUpperLimit = tmp;
            offset += 4;
            memcpy(&tmp, buf + offset, 4);
            AlarmLowerLimit = tmp;
            offset += 4;

            break;
        }
        default:
            return -1;
        }

        AppWarnParamHandle(dh->DataType, AlarmUpperLimit, AlarmLowerLimit);

        params++;
    }

    return params;
}

int SENSOR_PRTL_SetCommParam(const uint8_t* buf, uint16_t len)
{
    uint16_t offset = 0;
    uint8_t params = 0;

    while( offset < len)
    {
        uint32_t l;
        sensor_data_head_t* dh = (sensor_data_head_t*)(buf + offset);

        //参量类型是否有效
        if (dh->DataType < 1 || dh->DataType > sizeof(gdatatype))
            return -1;

        offset += sizeof(sensor_data_head_t);

        switch(dh->LengthFlag)
        {
        case 0: //4字节数据
            l = 4;
            break;
        case 1: //数据长度字段占用1个字节
            l = buf[offset++]; //数据长度
            break;
        case 2:
            memcpy(&l, buf + offset, 2);
            offset += 2;
            break;
        case 3:
            memcpy(&l, buf + offset, 3);
            offset += 3;
            break;
        }

        //检查长度是否符合规约
        if ( l % SENSOR_PARAM_LEN(dh->DataType) != 0)
            return -1;

        switch(gdatatype[dh->DataType-1])
        {
        case 0: //不能处理的参量类型
            return -1;
        case 1: //1个字节整形数据的参量类型
        {
            AppCommParamHandleU8(dh->DataType, buf + offset, l);
            offset += l;

            break;
        }
        case 2: //2字节整形数据的参量类型
        {
            AppCommParamHandleU16(dh->DataType, (uint16_t*)(buf + offset), l >> 1);
            offset += l;

            break;
        }
        case 3: //4字节整形数据的参量类型
        {
            AppCommParamHandleU32(dh->DataType, (uint32_t*)(buf + offset), l >> 2);
            offset += l;

            break;
        }
        case 4: //float数据的参量类型
        {
            AppCommParamHandleFloat(dh->DataType, (float*)(buf + offset), l >> 2);
            offset += l;

            break;
        }
        default:
            return -1;
        }

        params++;
    }

    return params;
}

//控制响应发送
static void CtrlAckSend(uint8_t* buf, uint16_t len)
{
    sensor_prtl_head_t* h = (sensor_prtl_head_t*)buf;

    h->PacketType = SENSOR_PACK_TYPE_CTRLACK;
    uint16_t crc2 = SENSOR_PRTL_CRC(buf, len);

    buf[len++] = crc2 >> 8;
    buf[len++] = crc2;
#ifdef SENSOR_MCU
    AT_SendData(buf, sSensorDataLen);
#else
    nwk_senddata(false, buf, len);
#endif
}

//控制报文解析
int SENSOR_PRTL_CtrlDecode(sensor_prtl_head_t* h, const uint8_t* buf, uint16_t len)
{
    sensor_prtl_ctrl_t* ctrhead = (sensor_prtl_ctrl_t*)(buf + sizeof(sensor_prtl_head_t));

    if (len <= sizeof(sensor_prtl_head_t) + 2 + sizeof(sensor_prtl_ctrl_t))
        return -1;

    switch(h->PacketType)
    {
    case SENSOR_CTRL_TYPE_REQMON:       //请求监测数据
        break;
    case SENSOR_CTRL_TYPE_REQTIME:      //请求校时
        break;
    case SENSOR_CTRL_TYPE_SETTIME:      //时间查询/设置
        break;
    case SENSOR_CTRL_TYPE_SETCOMM:      //通用参数查询/设置
    {
        uint16_t offset = sizeof(sensor_prtl_head_t) + sizeof(sensor_prtl_ctrl_t);
        uint16_t crc1, crc2;
        int ret = SENSOR_PRTL_CheckDataCotent(h, buf + offset, len - offset - 2, false);

        if (ret <= 0)
            return -1;

        crc2 = SENSOR_PRTL_CRC(buf, offset + ret);
        crc1 = buf[offset + ret] << 8;
        crc1 += buf[offset + ret + 1];

        if (crc1 != crc2)
            return -1;

        if (ctrhead->RequestSetFlag)
            SENSOR_PRTL_SetCommParam(buf + offset, ret);
        offset += ret;

        CtrlAckSend((uint8_t*)buf, offset);

        return offset + 2;
    }
    case SENSOR_CTRL_TYPE_SETWARN:      //告警参数查询/设置
    {
        uint16_t offset = sizeof(sensor_prtl_head_t) + sizeof(sensor_prtl_ctrl_t);
        uint16_t crc1, crc2;

        int ret = SENSOR_PRTL_CheckDataCotent(h, buf + offset, len - offset - 2, true);

        if (ret <= 0)
            return -1;

        crc2 = SENSOR_PRTL_CRC(buf, offset + ret);
        crc1 = buf[offset + ret] << 8;
        crc1 += buf[offset + ret + 1];

        if (crc1 != crc2)
            return -1;

        if (ctrhead->RequestSetFlag)
            SENSOR_PRTL_SetWarnParam(buf + offset, ret);

        offset += ret;
        CtrlAckSend((uint8_t*)buf, offset);

        return offset;
    }
    case SENSOR_CTRL_TYPE_SETID:        //ID查询/设置
    {
        uint16_t offset = sizeof(sensor_prtl_head_t) + sizeof(sensor_prtl_ctrl_t) + 6;
        uint16_t crc1, crc2;
        uint8_t* id = (uint8_t*)buf + sizeof(sensor_prtl_head_t) + sizeof(sensor_prtl_ctrl_t);

        crc2 = SENSOR_PRTL_CRC(buf, offset);
        crc1 = buf[offset++] << 8;
        crc1 += buf[offset++];

        if (crc1 != crc2)
            return -1;

        if (ctrhead->RequestSetFlag)
        {
            nwk_setid(id);
        }
        else
        {
            nwk_getid(id);
        }

        CtrlAckSend((uint8_t*)buf, offset - 2);

        return offset;
    }
    case SENSOR_CTRL_TYPE_RESET:        //传感器复位
    {
        uint16_t offset = sizeof(sensor_prtl_head_t) + sizeof(sensor_prtl_ctrl_t);
        uint16_t crc1, crc2;

        crc2 = SENSOR_PRTL_CRC(buf, offset);
        crc1 = buf[offset++] << 8;
        crc1 += buf[offset++];

        if (crc1 != crc2)
            return -1;

        SoftReset();

        return offset;
    }
    default:
        return -1;
    }

    return 0;
}

//传感器规约解析
int SENSOR_PRTL_Decode(const uint8_t* buf, uint16_t len)
{
    sensor_prtl_head_t* h = (sensor_prtl_head_t*)buf;

    if (len <= sizeof(sensor_prtl_head_t) + 2)
        return -1;

    switch(h->PacketType)
    {
    case SENSOR_PACK_TYPE_MONACK:
        break;
    case SENSOR_PACK_TYPE_WARNACK:
    {
        uint16_t offset = sizeof(sensor_prtl_head_t);
        uint16_t crc1, crc2;
        uint8_t ack = buf[offset++];

        crc2 = SENSOR_PRTL_CRC(buf, offset);

        crc1 = buf[offset++] << 8;
        crc1 += buf[offset++];

        if (crc1 != crc2)
            return -1;

        AppWarnAckHandle(ack == 0xff);

        return offset;
    }
    case SENSOR_PACK_TYPE_CTRL:
        SENSOR_PRTL_CtrlDecode(h, buf, len);
        break;
    case SENSOR_PACK_TYPE_FRAGACK:
    {
        uint16_t offset = sizeof(sensor_prtl_head_t);
        uint16_t crc1, crc2;
        sensor_prtl_frag_t ack;

        memcpy(&ack, buf + offset, 2);
        offset += 2;

        crc2 = SENSOR_PRTL_CRC(buf, offset);
        crc1 = buf[offset++] << 8;
        crc1 += buf[offset++];

        if (crc1 != crc2)
            return -1;

        AppFragAckHandle(ack.sseq, ack.pseq);

        return offset;
    }
    default:
        return -1;
    }

    return len;
}
